#!/usr/bin/env python

import rospy
import socket
import json
from sensor_msgs.msg import NavSatFix

# 全局变量存储UDP Socket
udp_socket = None

# 初始化UDP Socket
def init_udp_socket():
    global udp_socket
    try:
        udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)  # 设置 SO_REUSEADDR
        udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)  # 启用广播权限
        rospy.loginfo("UDP socket initialized")
    except Exception as e:
        rospy.logerr(f"Failed to initialize UDP socket: {e}")

# GPS数据回调函数
def gps_callback(data):
    global udp_socket
    if udp_socket is None:
        rospy.logwarn("UDP socket not initialized, skipping data send")
        return

    gps_data = {
        "latitude": data.latitude,
        "longitude": data.longitude,
        "altitude": data.altitude,
        "timestamp": data.header.stamp.to_sec()  # 将时间戳转换为秒
    }
    json_data = json.dumps(gps_data)

    try:
        # 发送数据到子网广播地址
        broadcast_address = '192.168.31.255'  # 子网广播地址
        udp_socket.sendto(json_data.encode('utf-8'), (broadcast_address, 12345))
        rospy.loginfo(f"Sent GPS data via UDP broadcast: {json_data}")
    except Exception as e:
        rospy.logerr(f"Failed to send data via UDP: {e}")

if __name__ == '__main__':
    # 初始化ROS节点
    rospy.init_node('gps_sender', anonymous=True)

    # 初始化UDP Socket
    init_udp_socket()

    # 订阅GPS话题
    # rospy.Subscriber('/mavros/global_position/global', NavSatFix, gps_callback)
    rospy.Subscriber('my_gps/fix', NavSatFix, gps_callback)

    # 保持节点运行
    rospy.spin()
